﻿#include "matrixd.h"
#include "memath.h"


void Matrix3d::multiplyPoint3D(double v[], double out[])
{
    double v1 = v[0];
    double v2 = v[1];
    double v3 = v[2];

    out[0] = v1*matrix[0][0] + v2*matrix[0][1] + v3*matrix[0][2];
    out[1] = v1*matrix[1][0] + v2*matrix[1][1] + v3*matrix[1][2];
    out[2] = v1*matrix[2][0] + v2*matrix[2][1] + v3*matrix[2][2];
}


void Matrix4d::zero(double elements[])
{
    for (int i = 0; i < 16; i++)
    {
        elements[i] = 0.0;
    }
}

void Matrix4d::identity()
{
    for ( unsigned row = 0; row < rows; ++row )
    {
      for ( unsigned column = 0; column < columns; ++column )
          if ( row == column )
              matrix[ row ][ column ] = 1.0;
          else
              matrix[ row ][ column ] = 0.0;
    }
}

void Matrix4d::identity(double elements[16])
{
    elements[0] = elements[5] = elements[10] = elements[15] = 1.0;
    elements[1] = elements[2] = elements[3] = elements[4] =
            elements[6] = elements[7] = elements[8] = elements[9] =
            elements[11] = elements[12] = elements[13] = elements[14] = 0.0;
}

void Matrix4d::translate(double t[])
{
    matrix[0][3] = t[0];
    matrix[1][3] = t[1];
    matrix[2][3] = t[2];
}

void Matrix4d::multiplyPoint3D(double v[3], double out[3])
{
    double v1 = v[0];
    double v2 = v[1];
    double v3 = v[2];
    double v4 = 1;

    out[0] = v1*matrix[0][0] + v2*matrix[0][1] + v3*matrix[0][2] + v4*matrix[0][3];
    out[1] = v1*matrix[1][0] + v2*matrix[1][1] + v3*matrix[1][2] + v4*matrix[1][3];
    out[2] = v1*matrix[2][0] + v2*matrix[2][1] + v3*matrix[2][2] + v4*matrix[2][3];
    // out[3] = v1*matrix[3][0] + v2*matrix[4][1] + v3*matrix[3][2] + v4*matrix[3][3];
}

void Matrix4d::multiplyPoint3D(MePoint3D &p, MePoint3D &outP)
{
    Matrix4d::multiplyPoint3D(p.v, outP.v);
}

MePoint3D Matrix4d::multiplyPoint3D(MePoint3D &p)
{
    MePoint3D outP;
    Matrix4d::multiplyPoint3D(p.v, outP.v);
    return outP;
}

void Matrix4d::multiplyPoint4D(double v[4], double out[4])
{
    double v1 = v[0];
    double v2 = v[1];
    double v3 = v[2];
    double v4 = v[3];

    out[0] = v1*matrix[0][0] + v2*matrix[0][1] + v3*matrix[0][2] + v4*matrix[0][3];
    out[1] = v1*matrix[1][0] + v2*matrix[1][1] + v3*matrix[1][2] + v4*matrix[1][3];
    out[2] = v1*matrix[2][0] + v2*matrix[2][1] + v3*matrix[2][2] + v4*matrix[2][3];
    out[3] = v1*matrix[3][0] + v2*matrix[4][1] + v3*matrix[3][2] + v4*matrix[3][3];
}

Matrix4d Matrix4d::getInverseHomogeneous() const
{
    // 参考 MeTransform::invertRotMatrix(Matrixd &mIn, Matrixd &mOut)
    /**
      [
          nx ox ax px
          ny oy ay py
          nz oz az pz
          0  0  0  1
      ]
      to (p = (px, py, pz), n = (nx, ny, nz), ... )
      [
          nx ny nz -p.n
          ox oy oz -p.o
          ax ay az -p.a
          0  0  0  1
      ]
            */
    // the inverse of the homogeneous transformation matrix
    Matrix4d mOut;

    // first row
    mOut.put(0, 0, matrix[0][0]);
    mOut.put(0, 1, matrix[1][0]);
    mOut.put(0, 2, matrix[2][0]);
    mOut.put(0, 3, -matrix[0][0]*matrix[0][3] - matrix[1][0]*matrix[1][3] - matrix[2][0]*matrix[2][3]);


    // second row
    mOut.put(1, 0, matrix[0][1]);
    mOut.put(1, 1, matrix[1][1]);
    mOut.put(1, 2, matrix[2][1]);
    mOut.put(1, 3, -matrix[0][1]*matrix[0][3] - matrix[1][1]*matrix[1][3] - matrix[2][1]*matrix[2][3]);

    // third row
    mOut.put(2, 0, matrix[0][2]);
    mOut.put(2, 1, matrix[1][2]);
    mOut.put(2, 2, matrix[2][2]);
    mOut.put(2, 3, -matrix[0][2]*matrix[0][3] - matrix[1][2]*matrix[1][3] - matrix[2][2]*matrix[2][3]);

    // forth row
    mOut.put(3, 0, 0.0);
    mOut.put(3, 1, 0.0);
    mOut.put(3, 2, 0.0);
    mOut.put(3, 3, 1.0);

    return mOut;
}

QString Matrix4d::toQString() const
{
    return QString::asprintf("[%lf %lf %lf %lf]\n[%lf %lf %lf %lf]\n[%lf %lf %lf %lf]\n[%lf %lf %lf %lf]\n",
                             matrix[0][0], matrix[0][1],matrix[0][2],matrix[0][3],
                             matrix[1][0], matrix[1][1],matrix[1][2],matrix[1][3],
                             matrix[2][0], matrix[2][1],matrix[2][2],matrix[2][3],
                             matrix[3][0], matrix[3][1],matrix[3][2],matrix[3][3]);
}

Matrix4d Matrix4d::fromString(const QString &matrixString)
{
    Matrix4d m;
    sscanf(matrixString.toLatin1(), "[%lf %lf %lf %lf]\n[%lf %lf %lf %lf]\n[%lf %lf %lf %lf]\n[%lf %lf %lf %lf]\n",
                                     &m(0,0), &m(0,1), &m(0,2), &m(0,3),
                                     &m(1,0), &m(1,1), &m(1,2), &m(1,3),
                                     &m(2,0), &m(2,1), &m(2,2), &m(2,3),
                                     &m(3,0), &m(3,1), &m(3,2), &m(3,3));
    return m;
}

Matrix4d Matrix4d::fromString(const std::string &matrixString)
{
    Matrix4d m;
    sscanf(matrixString.c_str(), "[%lf %lf %lf %lf]\n[%lf %lf %lf %lf]\n[%lf %lf %lf %lf]\n[%lf %lf %lf %lf]\n",
                                     &m(0,0), &m(0,1), &m(0,2), &m(0,3),
                                     &m(1,0), &m(1,1), &m(1,2), &m(1,3),
                                     &m(2,0), &m(2,1), &m(2,2), &m(2,3),
                                     &m(3,0), &m(3,1), &m(3,2), &m(3,3));
    return m;
}


void MatrixUtil::convertTinyToZero(Matrixd &m)
{
    convertTinyToZero(&m);
}

void MatrixUtil::convertTinyToZero(Matrixd *m)
{
    unsigned i = 0;
    unsigned j = 0;
    for(i = 0; i < m->getRows(); i++) {
        for (j = 0; j< m->getColumns(); j++) {
            if (ME_IS_ZERO(m->get(i,j))) {
                 m->put(i, j, 0);
            }
        }
    }
}

bool MatrixUtil::isMatrixdEqual(const Matrixd &mA, const Matrixd &mB, const double &eps)
{
    bool isEqual = true;
    for ( unsigned row = 0; row < mA.getRows(); ++row ) {
        for ( unsigned column = 0; column < mA.getColumns(); ++column ) {
            if ( abs(mA.get(row, column) - mB.get(row, column)) > eps ) {
                isEqual = false;
            }
        }
    }

    return isEqual;
}


//QString MatrixUtil::matrixdToString(const Matrixd &m)
//{
//    return QString::asprintf("[%lf %lf %lf %lf]\n[%lf %lf %lf %lf]\n[%lf %lf %lf %lf]\n[%lf %lf %lf %lf]\n",
//                                     m(0,0), m(0,1),m(0,2),m(0,3),
//                                    m(1,0), m(1,1),m(1,2),m(1,3),
//                                     m(2,0), m(2,1),m(2,2),m(2,3),
//                                     m(3,0), m(3,1),m(3,2),m(3,3));
//}

//Matrixd MatrixUtil::stringToMatrixd(const QString &matrixString)
//{
//    Matrixd m;
//    sscanf(matrixString.toLatin1(), "[%lf %lf %lf %lf]\n[%lf %lf %lf %lf]\n[%lf %lf %lf %lf]\n[%lf %lf %lf %lf]\n",
//                                     &m(0,0), &m(0,1), &m(0,2), &m(0,3),
//                                     &m(1,0), &m(1,1), &m(1,2), &m(1,3),
//                                     &m(2,0), &m(2,1), &m(2,2), &m(2,3),
//                                     &m(3,0), &m(3,1), &m(3,2), &m(3,3));
//    return m;
//}
